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I. 



INTRODUCTION 



Autonomous vehicles that are suitable for use in both 
naval and commercial operations, have unique mission 
requirements and dynamic characteristics. In particular, they 
are required to be highly maneuverable and very responsive as 
they operate in obstacle avoidance and object recognition 
tasks. The need therefore, arises to maintain accurate path 
keeping in confined spaces under the influence of steady and 
time varying external excitation. The primary vehicle 
guidance system is based on heading or turning rate commands 
that are generated based on a specified geographical sequence 
of desired way points. These guidance commands are then 
passed to the vehicle controller which attempts to deliver the 
commanded heading and/or heading rate of change by an 
appropriate use of the vehicle control surfaces [Ref.l]. For 
vehicle operations in confined spaces the way point sequence 
must be very dense so that satisfactory path accuracy is 
maintained. 

One efficient way of maneuvering through a given way point 
sequence is by using a line of sight guidance law which 
commands a heading angle that is directly related to the line 
of sight angle between the vehicle position and a desired 
destination point. The vehicle controller is then an 
orientation control law which delivers the commanded heading. 
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Previous studies [Ref. 2], have demonstrated that this 
scheme is guaranteed stable only if the way point separation 
is above some critical value. This conclusion is true 
regardless of the particular form of the line of sight 
guidance or the heading control law used. In this work we 
analyze the turning rate guidance and control problem where 
the guidance law demands a specific yaw rate response from the 
controller. A linear state feedback with a feedforward term 
[Ref. 3], control law is used, while two different guidance 
schemes are considered. The first is a cross track error 
guidance law which is very popular in land based robotic 
applications [Ref. 4]. The second is a proportional guidance 
law which is based on the line of sight angle between the 
vehicle and a target point. Stability analysis is performed 
and numerical integrations are used to confirm the theoretical 
results. All numerical computations in this work are 
performed for the Naval Postgratuate School autonomous 
vehicle, for which a complete set of geometric properties and 
hydrodynamic characteristics is available [Ref. 5]. 
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II. EQUATIONS OF MOTION 



Restricting our attention to the horizontal plane, the 
mathematical model consists of the nonlinear sway 
(translational motion parallel to the vehicle longitudinal 
axis) and yaw (rotational motion about the vertical axis) 
equations of motion. In a moving coordinate frame fixed at 
the vehicle's geometrical center (see Fig.l), Newton's 
equations of motion are: 





( 1 ) 


I^r+mx^(\) + ur) =N, 


( 2 ) 



where m is the mass of the vehicle , I2 its moment of inertia 
with respect to the vertical axis, u the forward velocity, Xq 
the coordinate of the vehicle center of gravity with respect 
to its centroid, "U and r are the relative sway and yaw 
velocities of the moving vehicle with respect to the water; 
and Y, N represent the total excitation sway force and yaw 
moment, respectively. 

Following standard vehicle maneuvering assumptions, these 
forces can be expressed as the sum of quadratic drag terms and 
first order polynomials in v and r with constant coefficients. 
In this way the nonlinear equations of motion in the 
horizontal plane become 
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m[\}+ur+Xf,t) =YfZ + Y^v+Y^ur + Y^uv+ 



+ Yf^u^d-j'Cjyh(^) (u+£r) |o+5r|d5 



( 3 ) 



*mx^, ( i) + ur ) =Nft+N^v+Nj.ur+NyUV+N^ u^b- 



-0.5cj'Q>/i(O (w+5r) |i>+5r|^d5 (^) 

In equation 3 the terms Y^., represent the change in the 
lateral force due to unit changes in the angular acceleration 
r and the lateral translational acceleration v, respectively. 
Likewise, the terms Y^, Y^ represent the change in the lateral 
force due to unit changes in the corresponding velocities v 
and r. The terms N^., N^, N^, in equation 4 are defined 

similarly. Ct, is the drag coefficient and 5 is the rudder 
angle. The NPS AUV II is equipped with both stern and bow 
rudder which are identical in size and are deflected in 
opposite directions for maximum maneuverability. In other 
words 

b,=b. 6j,= -6 (5) 

where 5, is the stern rudder angle, and 5t, the bow rudder 
angle. The cross flow integral drag terms in the equations of 
motion become important for hovering operations or low speed 
maneuvering, whereas at high speeds u the steering response is 
predominantly linear. 

To complete the model, we need the expressions for the 
vehicle yaw rate 
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t=r , 




(6) 


and the inertial position rates 






x=ucosT - vsinT , 




(7) 


y=usinY + vcosT , 




(8) 


where 5 , is the vehicle heading angle as shown in Figure 1 . 
Although it is recognized that the steering gear is both rate 
and angle limited, steering gear dynamics are not included in 



the formulation since they are much faster than the dynamics 
of a turning vehicle. The methods presented in the following 
sections can easily accommodate such modifications if desired. 

The surge velocity u is clearly affected during the trim 
due to the added drag in turning. For the purposes of this 
study, it is assumed to be constant. This is a valid 
approximation since experimental experience has shown that the 
propulsion control law is in general capable of keeping the 
forward speed u relatively constant at the commanded value. 
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y 




figure 1. Vehicle Geometry and Definitions of Symbols. 
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III. CONTROL LAW DESIGN 



A. LINEARIZATION 

As was mentioned in the previous chapter, during regular 
cruising operations at about 2 ft/sec forward speed, the 
nonlinear term in the equations of motion 3 and 4 are small 
and, therefore, effective control can be maintained by using 
their linear form 



/r?(i) + ur+Xgf ) =y^r + y^v+yj.ur+yyuv+y^u^d . (9) 

(v+ur) =N^r+N^v+N^ur+N^uv+N^u 26. (10) 

After some algebra, equations 9 and 10 can be put into 
state space form as 



V = a^^uv+ay2^r+b^u^b , 


(11) 


r = a 2 ^uv+a 22 ur+b 2 u^b , 


(12) 



where 

Da^2 = (m-Y^) -{mx^-Yf) (N^-mx^) , 

£>321 = {w-Y^)N^-(mXg-N^) Y^, 

Da^2 - (m-Y^) (ATj.-/nX(.) -(mx^-Nv) {Y^-m) , 
Db^ = {I,-N,)Y^-{mx^-Y,)N^, 
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Db^ = iw- Y^) N(, - i rnx^-Nv) Fj , 

D = ilz-N^) iiri-Y^) -iiux^-Y^) {mx^-N^) . 

Equations 11 and 12 describe the lateral dynamics of the 
vehicle for small motions. 

B . FEEDBACK CONTROL 

A linear rudder feedback control law based on equations 11 
and 12 has the form 

b = k^v-^kj.r, (13) 

where Jc,, k,. are the feedback gains. By substituting equation 
13 into 11 and 12 we get the closed loop dynamics equations 

v= {a^^u+b.^u'^k^) v-^ {a^^u-^h-^u^kj.) r , (14) 

r= (a^iU+b^u^k^,) v+ (a^zU+b^u^kj.) r (15) 

The characteristic equation of 14 and 15 is 

k^+A^X+A^ = 0, (16) 

where 

•^1 = [a^^+a 22 + (b^k^+b 2 k^) u] u, 

^2= it>2a^^~b^a2:^) Uic^] . 

Now the desired closed loop characteristic equation is 
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(17) 



A.^+a^X+a2 = 0 

By equating the coefficients of eqs.l6 and 17 we get the 
following system of linear equations 

= “*ai-(aii+a22) U, (18) 

^v^^22^1~^12^2^ ~ ^2~ (19) 

- (a^^a22”3i2^2i^ 

Equations 18 and 19 then determine the gain k^, in the 

feedback control law (Eq.l3). 

The coefficients Oi, tt 2 of the desired characteristic 
equation 17 can be specified according to any standard pole 
placement design technique. In this work we decide to use the 
controller time constant, T^, as the parameter that specifies 
the coefficients Oi, dj. In this way the desired 

characteristic equation is 

U + A)=o or + = (20) 

tI 



and comparing with eq.l7 we see that 



a 



1 





( 21 ) 



Specification of a controller time constant T^, then 
determines the feedback gains k^, k^ uniquely. 
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C . FEEDFORWARD CONTROL 

The control law (Eq.l3) guarantees stability of v = r = o 
of equations 11 and 12, in other words straight line motion at 
an arbitrary heading. When the commanded angular velocity r^, 
is nonzero the control law is slightly modified to 

6 ^ k^v+k^(r-r^) +k^r^, ( 22 ) 

where r^, is the commanded turning rate and k^, is the 
feedforward gain. This is computed based on steady state 
accuracy requirements. At steady state, equations 11 and 12 
yield 

^ ^ (23) 

where 






•^1^22 ^ 2^12 
•^ 2^11 “-^ 1^21 



g _ ^ 21^12 ^ 11^22 

^ ^2^11-^1^21^ 



( 24 ) 



Substituting equations 23 and 24 into equation 23, and 
requiring that r = r<. at steady state, we can solve for k^. 



kc = ' 

and the control law (Eq.22) becomes 



( 25 ) 



6=k^v+k^r- {k^-b^+k^v^) 



( 26 ) 



Substituting the values of k^, 6„, k^, v^ in (2 6) , we can 

finally write the control law (Eq.26) in the form 
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( 27 ) 



6 = k^v+kj-i-k^a^r^ . 

where 



^0 



1 



( 28 ) 



With the above feed forward gain the control law is 
complete. It should be mentioned that all gains K,, k^., kg 
depend explicitly on the forward speed u and are, therefore, 
continuously updated every time a different forward speed is 
commanded. 



D . RESULTS 

The response of the control law (Eq.27) applied to the 
system of Equations 11 and 12 is shown in Figure 2, where the 
commanded turning rate is 0.1 rad/sec. It can be seen that 
the actual vehicle turning rate r converges to its commanded 
value r^, as expected. The gain were computed based on a 
selected time constant T^, = 1 dimensionless second. Time is 
nondimensionalized here with respect to the vehicle forward 
speed u = 2 ft /sec and the vehicle length L = 7.3 ft. 
Therefore, one dimensionless second corresponds to 3.65 
seconds of real time and this is verified by the simulation 
results of Figure 2 (the response drops about 60% every time 
constant) . 

The response for different time constants (in 
dimensionless seconds) is shown in Figure 3 where it can be 
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result in slower vehicle 



seen that higher values of T^, 
response as expected. Similar is the rudder angle response as 
shown in Figure 4. Smaller values of T^, result in tighter 
control and more rudder activity. 

E . ACCURACY 

The feedforward gain kg computed from eq.28 ensures that 
the steady state turning rate r equals the commanded value r<, 
for the linear system of equations 11 and 12. To analyze the 
effects of nonlinearities we start with the nonlinear 
equations of motion 3 and 4 and we write them in the form 

v = aj^^uv+aj^2^r+b^u ^5 + d^(v, r), (29) 

f = a.^ 2 ^v+a^^ui+b^u'^b-^dj.{v, r) , (30) 

Dd^iv, r) =-0 .5qCd[ Uz-N^) Iz] , 

Dd^iv, r) = -0 . 5QCp[ iN^-mx^) Iz] . 

Uz-N^) (m-Y^) - imx^-Y^) imx^-N^) , 

(u + U) \v^lr\d^ , 

The values of v and r at steady state can then be computed 

from 
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Tc=l 




Figure 2. Turning Rate Response for 1 . 
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Figure 3. Turning Rate Response for Different Values of T,.. 
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Figure 4. Rudder Angle Time Histories for Different Values of T^. 
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a.^^uv+a.^2^r+h^u^b+d^[v,T)=0 , 



( 31 ) 



a2^uv+a22ur+b2u^6+dr{v,r)=0 , 



(32) 



b-k^v-kj.i-^k.QCt2r^=-0 . 



0'*2^ c 



( 33 ) 



The solution to these equations is not r = unless = 
dr = 0 . This is demonstrated by the time simulation of Figure 
5, for which the quadratic drag coefficient Cu was set equal 
to 0.5. It can be seen that the vehicle turning rate develops 
a nonzero steady state error with respect to the commanded 
value of 0.1 rad/sec. As the controller time constant T^, is 
decreased, the control law becomes tighter and this steady 
state error could not become zero due to uncertainties in the 
vehicle hydrodynamic description and various unmodeled 
dynamics. Even if ko were determined from equations 31 to 33 
such that r = r^, the uncertainties involved in the 
determination of the hydrodynamic coefficients in the 
equations of motion would cause the actual value of r to be 
different than the commanded value r^. One way to ensure 
steady state accuracy is to abandon the use of the feedforward 
gain k,, and to introduce integral control. This possibility 
is examined in the next section. 
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r (rad/sec) 




Figure 5. Turning Rate Response for Different Values of and for 
Nonzero Drag Coefficient. 
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F . INTEGRAL CONTROL 

An alternative way to ensure stability and convergence to 
the commanded value r^ is to introduce integral control in the 
form 



b=k^v+k^(r-r^) {r-r^) dt (34) 

In that case, the system of state equations is augmented 
by the additional 

r, = r-r^ (35) 

In order to compute the gains k^, k^, kj we set r„ = 0 and 
substitute the control law 

6=k^v+kj.r+kjrj , (36) 

into equations 11 and 12. The resulting closed loop system is 

v~= (a^^u+kj:>^u^) v+ (aj^^u+kjp^u^) r+kjh^u^r j. 
r= {a 2 iU+kjD 2 U^) v+ 



(37) 

(38) 






(39) 



The characteristic equation of equations 37 to 39 can be 
computed as 



X^+AiA^+a2A.+a3 = 0 



(40) 



where 
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■'^2 ^^ H '^22 ^ 21 ^ 12 ^ ^ ^ ^^ H '^2 ^ 21 '^!^ ^ 

•^3 ~ ( ^ 11-^2 ~‘^ 21'^1 ^ • 

If the desired characteristic equation is 

A^+a^A,^+a2A+a3=0 (41) 

by equating the coefficients of equations 40 and 41 we get the 
following system of linear equations which determines the 
three gains k^, k^., kj: 

kjD^u^+k^2^^ = -a^~ (aj^j^+a22) u , 

( ^ 22 ^ 1 ~ ^ 12 ^ 2 ^ ^ ^ ("® 11‘^2 ~^ 21^1 ) ^ ~^ 2 ^^ r ^ 2 ^^ 

“ ^^11^22~^21^12^ , 

kiia^^bz-az^b^) u^=a^ . 

For a time constant and an integrator time constant Tj, 
the desired characteristic equation becomes 

(X+A)'(X + A) =0 r 

or 



A3+( A+A)X2+( Ju+, 



T T 

b ji ^ 



)A- 



TlTr 



=0 



so that 
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2 

T T 

^ i-‘-c 



With these choices the determination of the gain k,, k,., 
kj, is complete. A numerical simulation for r^ = 0.1 rad/sec. 
and with = 0.5 is presented in Figure 6. The time 
constants are = 1 and Tj assumed the three different values 
shown in the figure. It can be seen that despite the 
nonlinear drag terms, steady state accuracy is achieved. 
Since the response of systems compensated by integral control 
action tends to be more oscillatory, in this work we will use 
the previous feedforward control . As we show in the next 
chapter, steady state accuracy is guaranteed once the control 
law is combined with a suitable guidance. 
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integral control 




Figure 6. Turning Rate Response for 1^=1^ Different Values of Tj^ 
Nonzero Drag Coefficient^ and Use of Integral Control. 
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IV. GUIDANCE LAW 



A. GENERAL 

The previously developed control law is able to deliver a 
vehicle commanded turning rate r^. In order to achieve path 
control to a commanded route in the horizontal plane, however, 
the commanded turning rate r^ must be appropriately selected. 
This constitutes the guidance law design. Two such guidance 
laws are described in the following, cross track error and 
proportional guidance. Without loss of generality, we assume 
that the commanded path is a straight line. This is not a 
very restrictive assumption since every smooth path can be 
discretized into a series of straight line segments as 
accurately as desired. 

B. CROSS TRACK ERROR GUIDANCE 

The guidance law is based solely on kinematics, whereas 
vehicle dynamics are handled by the control law. Guidance law 
development based on 

i = (42) 

y = usim|f , (43) 

where r^ is the commanded turning rate and the lateral 
velocity v is assumed to be zero in equation 43. 

Cross track error guidance is achieved by 
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( 44 ) 



By substituting equation 44 into equation 42 we get the 
closed loop guidance system 

i = ic^il;+ic^T|r , (45) 

y - usini|; , (46) 

The characteristic equation of equations 45 and 46 is 
obtained by making the small angle approximation sinvj/ = Y/ and 
is 



X^-k^X-kyU = 0 . (47) 

If the desired characteristic equation is 

A2 + P^X+P2 = 0 , (48) 

the guidance law gains k^, ky are obtained by equating the 
coefficients of equations 47 and 48 

k^ = -Pi , (49) 

k=-^. (50) 

^ u 

Analogously to the control law design, if the time control 
of the guidance law is selected to be T^, then equations 49 
and 50 result in 



k 



> 1 ' 




(51) 
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1 



( 52 ) 




Selection of then determines k^, ky directly. 

Although this development followed the small angle 
approximation, it is not difficult to see that negative values 
of k^ and ky will guarantee stability of the nonlinear system 
of equations 45 and 46. The associated total energy of the 
system is 

£■(1^ , t) =-^iif^-A:yU{l-cosi|;) , (53) 

which can be viewed as the sum of kinetic and potential 
energy. Using equation 45, this is written as 



E{\^ ,y) (k^i\i+kyy)^-kyU(l-cos\\i) . (54) 

We note that E(\j/^y) provides a Liapunov function for equations 
45 and 46, since E(0,0)=0 at the unique equilibrium 
(\(^, y) = ( 0 , 0) and E(Y, y)>0 for because ky<0 

Moreover, we have 



di}; dt dy dt d»Jr ^ dy^ 



(55) 



Evaluating the indicated partial derivatives we get 



dE 



-^ = ik^r^+kyy) k^-kyUsiny^ 



(56) 



dE_ 

dy 



ik^y\i+kyy) ky . 



(57) 
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Substituting equations 45, 46, 56 and 57 into equation 55 



we find 



E=k^ (ic^ilf+Ar^y) ^ 



( 58 ) 



which is negative. Therefore, Liapunov's theorem guarantees 
asymptotic stability of the nonlinear system (equations 45 and 
46) . 

C . PROPORTIONAL GUIDANCE 

A typical orientation based guidance law is pursuit 
guidance which is accomplished as follows: The commanded 
vehicle heading angle y,, equals the line of sight angle 
between the vehicle and a target point D located ahead of the 
vehicle and a constant preview distance d on the nominal 
straight line path as shown in Figure 1 . In other words 



In a proportional guidance scheme a time constant T is 
incorporated in the above line of sight equation 



and we arrive at the proportional guidance which is used here 
for path control 



tlf,, = -tan"^-^, . 

d 



( 59 ) 






( 60 ) 



rr-+t = -tan'^^ , 

d 



( 61 ) 
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( 62 ) 



C J.T rp ^ 



The linearized form of equation 62 is 



r = - — Ur- — V , 
Td 



(63) 



and by comparing it to equations 48 through 50 we can see that 
it corresponds to a guidance characteristic equation with 



Px = 




(64) 



P2 = 



u 

Td 



(65) 
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V. 



STABILITY 



A. SIMULATIONS 

In order to assess stability of the combined guidance and 
control law we proceed first by a series of numerical 
integrations. The simulations are based on the dynamical 
equations 3 and 4 and the kinematic equations 6 and 8. The 
rudder control law in the form of equation 27 is used, while 
the cross track error guidance (eq.44) is employed. Results 
are presented in terms of the lateral deviation y in ft versus 
time t in seconds. Figure 7 shows the vehicle response for 
control time constant Tj,=0 . 5 and guidance time constant Tg=l . 
It can be seen that the response is stable, although it 
exhibits slow convergence characteristics to the commanded 
path y=0 . To speed-up speed of response we could lower the 
guidance time constant T<j. The results of the simulation for 
T^=0.5 and Tq= 0.5 are presented in Figure 8. It can be seen 
that the convergence is now faster although the path overshoot 
is higher. If we maintain the same guidance time constant 
Tg=0.5 and increase the control time constant T^=2, the vehicle 
is unstable as demonstrated by Figure 9. The response 
exhibits now oscillatory characteristics or a limit cycle. 
The explanation for this phenomenon is that, in this case the 
control law is very slow and the rudder cannot follow the 
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commanded turning rates by the guidance law which is 
considerably faster, due to its smaller time constant. To 
remedy this instability we should either lower as in Figure 
8 demonstrated, or increase the value of Tq. The results of 
this simulation are shown in Figure 10, for which T^=2 and 
Tg=2 . The response exhibits now characteristics of a damped 
oscillation and is stable, although convergence is very slow 
and oscillatory. 

B . STABILITY 

The previous simulation results show that there exists a 
certain range of (T^, T^.) combinations that results in stable 
response. In order to compute this range and verify the 
numerical integration results, we use the linearized equations 
of motion 





i(r = r 


(66) 






(67) 




r=a2iuv+a22ur+jb2u^6 , 


(68) 




y=ut|r + v 


(69) 


the 


rudder control law 






b=k^v+k^r-k^a^i^, 


(70) 


and 


the turning rate guidance law 






r^^k^yif+kyV. 


(71) 
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In a compact vector notation the linearized equations of 
motion 66 through 71 are written as 

X=Ax, r,y] 

Motion stability is established by the eigenvalues of 
matrix A, if all eigenvalues have negative real parts the 
nominal straight line motion is dynamically stable, while if 
at least one eigenvalue of A is positive, the system is 
unstable. Writing out the characteristic equation of equation 
72 we get 

X^+BX^+Ck^+Dk+E=0 , ( 73 ) 

where 



S=a^ , 


(74) 


C=a2~ibi^2^^2^^i)^K°-2 ' 


(75) 


■D=a2pi+dia2p2 , 


(76) 


^=«2P2 ' 


(77) 



and 



di=- 



^ 1^22 

^^2^11-^1^21)^ 



(78) 



In the derivation of expressions 74 to 78 we have used 
equations 18 and 19 for the feedback gains, equation 28 for 
the feedforward gain, and equations 49 and 50 for the general 
guidance law gains. 
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If we apply Routh' s criterion to the fourth order equation 
73 we can find the following critical condition for stability 



BCD-B^E-D^>0 . 



(79) 



If we substitute equations 74 through 77 in equation 79, 
we find the following condition 



Equation 80 represents in algebraic form the critical 
condition for stability in terms of the control law 
characteristic equation coefficients (X^, Oj and the guidance 
law coefficients Pi, Pj. It can be used for both the cross 
track error and the proportional guidance schemes, if we 
employ the appropriate expressions for Pi, P 2 from Chapter IV. 

For the case of cross track error guidance, equation 80 
results in 



For a given guidance law time constant T^, equation 81 
specifies the critical control time constant for stability. 

Stability of the combined guidance and control scheme is 
guaranteed for values of that are less than the critical 
value computed from equation 81. For values above the 
critical value (eq.80) the system is unstable. In these 
cases, one pair of complex conjugate eigenvalues of matrix A 




(80) 



2 (b^+2b2T^u) uk^] {2Tg+d^) 

4T^^+{2Tq+c1^)^ 



(81) 
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in equation 72 has positive real parts, and as a result the 
response of the system is oscillatory. 

A plot of the critical stability condition (Eq.81) is 
shown in Figure 11. the response is stable for (T,., T,,) 
combinations that lie below the critical curve. The number 1 
through 4 on the figure correspond to the four numerical 
simulations that were presented in Figure 7 through 10, 
respectively. It can be seen that the simulation results agree 
with the stability analysis performed in this section. Point 
3 is located above the critical curve and the results in the 
unstable oscillatory response observed in Figure 9. 
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rigur® 7. Path Keeping Response for T^-O-S, Ts-l . 
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Figure 8. Path Keeping Response for T5=0.5, Ts-O . 5 . 
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Figure 9. Path Keeping Response for Tg—0 . 5 . 
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rigura 10. Path Keeping Response for T^=2, T(-«2 . 
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Figure 11. Critical T, Curve for Stability. 
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VI. 



CONCLUSIONS AND RECOMMENDATIONS 



The goal of this work, stability analysis of turning rate 
guidance and control for autonomous vehicles has been 
achieved. The control law was based on the dynamic equations 
in sway and yaw. The feedback gains were analytically 
computed by pole-placement techniques and the feedforward gain 
was evaluated based on the desired steady-state accuracy. 

The guidance laws that were utilized, cross-track error or 
proportional guidance, were based on the kinematics relations. 
It was found that unless the guidance and control laws were 
designed according to certain conditions, stability of the 
system was not guaranteed. These stability conditions were 
computed analytically and the results were verified by 
numerical simulations. Recommendations for future work 
include an analysis of the performance of turning-rate 
guidance and control from the point of view of sensitivity to 
sensor noise, state estimation, and disturbance rejection. 
Comparisons with other guidance and control laws, such as line 
of-sight guidance, should also be performed. 
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